﻿using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using AR.Drone.Client;
using AR.Drone.Client.Command;
using AR.Drone.Client.Configuration;
using AR.Drone.Data;
using AR.Drone.Video;


namespace isDrone
{
    public class ISDrone
    {

        public string IP = "192.168.1.1";
        public  DroneClient DroneClient;
        private bool _connection;
        public Video Video;
        public Telemetry Telemetry;
        public Input _input;
        public float TanKrenK = 0;
        public float RiskK = 1;
        public float AltK = 1;
        public string[] modes = new string[] { "Об.", "Yaw."};
        public bool mode = false;
        public bool magneto = false;
        private float modeangle = 0;
        public Boolean Connection { get { return _connection; } }

        public ISDrone()
        {
            DroneClient = new DroneClient(IP);  
            Video = new Video(this);
            DroneClient.VideoPacketAcquired +=Video.OnVideoPacketAcquired;
            DroneClient.StatusChange += StateWork;
            Telemetry = new Telemetry(this);
            _input = new Input();
            _input.StartEvent += Takeoff;
            _input.StopEvent += Land;
            _input.EmergencyEvent += Emergency;
            _input.UpdatEvente += Update;
            _input.Hover += Hover;
            _input.FlatTrim +=Flattrim;

            _input.VideoStop += VideoStop;
            _input.VideoStart += VideoStart;
            _input.SetMin += SetMin;
            _input.SetPlus += SetPlus;
            _input.Mode += SetMode;
            _input.Magneto += SetMagneto;

        }

        public void SetMin()
        {
            TanKrenK = TanKrenK - 0.1f;
            if (TanKrenK < 0) TanKrenK  = 0;
        }


        public void NewSettings()
        {
            
        }

        public void SetPlus()
        {
            TanKrenK = TanKrenK + 0.1f;
            if (TanKrenK >1) TanKrenK  = 1;
        
        }
        public void SetMode()
        {
            mode = !mode;
            modeangle = Telemetry.Date.Yaw;
        }


        public void SetMagneto()
        {
            magneto = !magneto;
        }
        public void Dispose()
        {
            DroneClient.VideoPacketAcquired -= Video.OnVideoPacketAcquired;
            DroneClient.StatusChange -= StateWork;
            DroneClient.Dispose();
            Video.Dispose();
        }

        private object lu = new object();
        public void Update(InputState state)
        {

            lock (lu)
            {

                FlightMode m = FlightMode.Progressive;

                float angle = Telemetry.Date.Yaw - modeangle;
                float kren = state.Kren*TanKrenK;
                float tangag = -state.Tangag*TanKrenK;

                if (mode)
                {

                    var v = SharpDX.Matrix.RotationY(angle);
                    SharpDX.Matrix ma =new SharpDX.Matrix();
                    ma[0,0] = kren;
                    ma[0,1] = 0.0f;
                    ma[0,2] = tangag ;
                   var  rr = ma * v;

                    kren = rr[0];

                    tangag = rr[2];
                }
        
                if (magneto)
                    DroneClient.ProgressWithMagneto( FlightMode.CombinedYaw,kren , tangag, state.Riskanie * RiskK, state.Gaz * AltK, 0.1f, 0.1f);
                else
                    DroneClient.Progress(m, kren, tangag, state.Riskanie * RiskK, state.Gaz * AltK);
            }
        }
        public void Connect()
        {     
          
            DroneClient.Start();
          
            System.Threading.Thread.Sleep(5000);
            if (DroneClient.IsConnected)
            {
                Settings settings = new Settings();
                DroneClient.AckControlAndWaitForConfirmation(); // wait for the control confirmation
                settings.Custom.SessionId = "ad1efdac";
                settings.Custom.ProfileId = "992f7f4f";
                settings.Custom.ApplicationId = "510acf97";
          settings.General.NavdataDemo = true;
                DroneClient.Send(settings);
                DroneClient.AckControlAndWaitForConfirmation();
              
        
            }

        }

        private Settings _settings;
        public void getSettings()
        {
            Task<Settings> configurationTask = DroneClient.GetConfigurationTask();
            configurationTask.ContinueWith(delegate(Task<Settings> task)
            {
                if (task.Exception != null)
                {
                    Trace.TraceWarning("Get configuration task is faulted with exception: {0}",
                        task.Exception.InnerException.Message);
                    return;
                }

                _settings = task.Result;
                UpdateSetting();
            });
            configurationTask.Start();
        }

        public void UpdateSetting()
        {
            if (_settings != null)
            {
                
            }
        }


        public void Flattrim()
        {
            DroneClient.FlatTrim();
        }

    
        public void Hover()
        {
            DroneClient.Hover();
        }


        public void Emergency()
        {
     
            DroneClient.Emergency();

        }

        public void Takeoff()
        {
            DroneClient.Takeoff();
        }

        public void Land()
        {
            DroneClient.Land();
        }

        public void VideoStart()
        {
            var settings = new Settings();
            settings.Custom.SessionId = "ad1efdac";
            settings.Custom.ProfileId = "992f7f4f";
            settings.Custom.ApplicationId = "510acf97";
            settings.Video.Codec = VideoCodecType.MP4_360P_H264_720P;
            DroneClient.Send(settings);
            DroneClient.AckControlAndWaitForConfirmation();
        }

        public void VideoStop()
        {
            var settings = new Settings();
            settings.Custom.SessionId = "ad1efdac";
            settings.Custom.ProfileId = "992f7f4f";
            settings.Custom.ApplicationId = "510acf97";
            settings.Video.Codec = VideoCodecType.H264_720P;
            DroneClient.Send(settings);
            DroneClient.AckControlAndWaitForConfirmation();
        }

   
        private void StateWork(object sender, EventArgs args)
        {
            if (DroneClient != null && _connection != DroneClient.IsConnected)
            {
                _connection = DroneClient.IsConnected;
                if (ConnectionEvent != null) ConnectionEvent(null, null);
            }

        }
        public event EventHandler ConnectionEvent;

        public void Reset()
        {
            DroneClient.ResetEmergency();
        }
    }
}
